//Motors node firmware for Team D.I.A.N.A
//Mattia Marenco
//matiapascal@gmail.com

#include <string.h>
#include <Wire.h>

#define CPR 33000 //per versione interpolata 4x

unsigned long time_count,time_flag=0,Ti,Tc=10*1000,Tm=5*1000;  //timer lettura dati, timer ultimo flag critico, timer inizio motori, tempo campionamento, tempo azionamento motori

float MaxLinearV[6]={16.96,33.92,67.86,169.64,678.58,1696.45};           //Maximum linear velocity standard[4]= 339.29[m/h]. ->678.58ct/s ->10rpm
float MaxAngularV[6]={0.5,1,2,5,40,150};                                  //Maximum angular velocity attorno a se stesso del rover[rpm]. (10rpm lineari vanno bene 10 rotazioni minuto, empiricamente)
int i;
float velocityL=0, velocityR=0;                           //Velocity of Left and Right wheels [ct/s].
byte cL=127, cR=127;                                       //Velocity command from i2c of Left and Right wheels.
float r=0.09;                                              //Wheel radius [m].
float t=2*3.14159*r;                                              //Track of the rover [m].
boolean folleR=false,folleL=false;
boolean flag_moto=false;
boolean MAF=false;
boolean MLM=false;

char risposta_ext[32];
byte risposta_ext_send[32];

char risposta[12];  
byte flag=0,flagC=0,Mo=0; //flag critico per segnalazione su i2c
int dati[4][16]={{0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},{0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},{0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},{0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}};
byte i2c[2][20]={{0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},{0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}};
int NUM_DATI;
unsigned long risposta_long;   
float risposta_float;


void setup() {
  Serial.begin(19200);         //serial communication initialization
  Serial1.begin(19200);
  Serial2.begin(19200);
  Serial3.begin(19200);
  analogReference(EXTERNAL);   //5V dal reference
  
  //Serial.println("RESET");
  
  //attachInterrupt(0, function, falling) //2 motori
  //attachInterrupt(1, function, falling) //3 master  //**********utilizzato ad ora
  pinMode(3, INPUT); //input no freewheel
  pinMode(5, INPUT); //freewheel NH lo si tiene premuto quindi non critico come tempo di lettura
//  digitalWrite(5,HIGH); //da togliere su pcb!
  pinMode(12, OUTPUT); //interrupt verso master
  pinMode(13, OUTPUT); //led parlo con motori + no input verso master
  pinMode(40, INPUT); //stop NH con fungo che si blocca quindi non critico come tempo di rilevazione (segnalarlo al master)
//  pinMode(42, OUTPUT); //M1 A
//  pinMode(43, OUTPUT); //M1 B
//  pinMode(44, OUTPUT); //M2 A
//  pinMode(45, OUTPUT); //M2 B
//  pinMode(46, OUTPUT); //M3 A
//  pinMode(47, OUTPUT); //M3 B
//  pinMode(48, OUTPUT); //M4 A
//  pinMode(49, OUTPUT); //M4 B
//  pinMode(50, OUTPUT); //wahdog timer M1 2.2s
//  pinMode(51, OUTPUT); //wahdog timer M2
//  pinMode(52, OUTPUT); //wahdog timer M3
//  pinMode(53, OUTPUT); //wahdog timer M4
  
  digitalWrite(12,LOW); //notifica reset su interrupt
  delay(10);
  digitalWrite(12,HIGH);

  time_count=0;

  digitalWrite(13,LOW); //abilita input da master

  Wire.begin(2);
  Wire.onRequest(requestEvent);
  Wire.onReceive(receiveEvent);
  
  char comando[13];     //output to serial communication string

  //motor startup attesa i2c
  delay(1000);

  digitalWrite(13,HIGH);   //disabilita richieste i2c
  
//  digitalWrite(50,HIGH); //resetta i wahdog
//  digitalWrite(51,HIGH);
//  digitalWrite(52,HIGH);
//  digitalWrite(53,HIGH);
//  delay(10);
//  digitalWrite(50,LOW);
//  digitalWrite(51,LOW);
//  digitalWrite(52,LOW);
//  digitalWrite(53,LOW);
  
  strcpy (comando,"MO=0");                //stop motor
  scrivi_seriale(comando,0);
  scrivi_seriale(comando,1);
  scrivi_seriale(comando,2);
  scrivi_seriale(comando,3);
    
  configurazione_iniziale(0);
  configurazione_iniziale(1);              //motor driver configuration                      
  configurazione_iniziale(2);              //motor driver configuration
  configurazione_iniziale(3);
  
  strcpy(comando,"MO=1");                 //start motor
  scrivi_seriale(comando,0);
  scrivi_seriale(comando,1);
  scrivi_seriale(comando,2); 
  scrivi_seriale(comando,3); 
  
  digitalWrite(13,LOW);
  
  i=5;      //setting initial velocity to 10rpm
} 

void loop() 
{
  char comando[13];
  char buffer [sizeof(long)*8+1];
  float convfactor=(CPR)/(3600*2*3.14159);
  int z;
  
  
//  digitalWrite(50,HIGH); //resetta i wahdog
//  digitalWrite(51,HIGH);
//  digitalWrite(52,HIGH);
//  digitalWrite(53,HIGH);
//  delay(10);
//  digitalWrite(50,LOW);
//  digitalWrite(51,LOW);
//  digitalWrite(52,LOW);
//  digitalWrite(53,LOW);
  
  
  if(digitalRead(5)==LOW)                     //freewheel
  {
      strcpy (comando,"MO=0");                //stop motor
      scrivi_seriale(comando,0);
      scrivi_seriale(comando,1);
      scrivi_seriale(comando,2);
      scrivi_seriale(comando,3);
      MAF=true;                               //sfrutto il flag di MAF per riabilitare i motori
  }
  
  if((cL!=127)||(cR!=127))
  {
   if(MAF){
      strcpy (comando,"MO=1");                //riabilita i motori se si è in MAF
      scrivi_seriale(comando,0);
      scrivi_seriale(comando,1);
      scrivi_seriale(comando,2);
      scrivi_seriale(comando,3);
      MAF=false;
      }

    velocityL=map(cL, 1, 255, -MaxLinearV[i], MaxLinearV[i]);     //velocità lineare in m/h
    velocityR=map(cR, 1, 255, -MaxLinearV[i], MaxLinearV[i]);
//    Serial.println(cL);
//    Serial.println(velocityL);
//    Serial.println(cR);
//    Serial.println(velocityR);
    velocityL=(-(velocityL/r)*convfactor);                         //velocità angolare in cts
    velocityR=(-(velocityR/r)*convfactor);
//    Serial.println(velocityL);
//    Serial.println(velocityR);
     
    
    digitalWrite(13,HIGH);
    
    if (folleL==false)
      {
      strcpy (comando,"JV=");                        //send the velocity command to motor driver
      strcat (comando,ltoa(int(velocityL),buffer,10));     //convert the velocity number to ASCII string in order to send it
      }
    else
      strcpy (comando,"MO=0");
    scrivi_seriale(comando,0);
    
    if (folleR==false)
      {
    strcpy (comando,"JV=");
    strcat (comando,ltoa(int(velocityR),buffer,10));     //convert the velocity number to ASCII string in order to send it
      }
    else
      strcpy (comando,"MO=0");    
    scrivi_seriale(comando,1);
    
    if (folleL==false)
      {
    strcpy (comando,"JV=");
    strcat (comando,ltoa(int(velocityL),buffer,10));     //convert the velocity number to ASCII string in order to send it
      }
    else
      strcpy (comando,"MO=0");    
    scrivi_seriale(comando,2);
    
    if (folleR==false)
      {
    strcpy (comando,"JV=");
    strcat (comando,ltoa(int(velocityR),buffer,10));     //convert the velocity number to ASCII string in order to send it
      }
    else
      strcpy (comando,"MO=0");    
    scrivi_seriale(comando,3);
    
    strcpy(comando,"BG");                          //start the motion
    if (folleL==false)
      scrivi_seriale(comando,0);
    if (folleR==false)
      scrivi_seriale(comando,1);
    if (folleL==false)
      scrivi_seriale(comando,2);
    if (folleR==false)
      scrivi_seriale(comando,3);
      
    Ti=millis();
    flag_moto=true;
    
    cL=127;
    cR=127;
    digitalWrite(13,LOW);
  }
  
  //operazione richieste via I2C ---------ne esegue una ogni ciclo loop----------
  if(i2c[0][0]>3) 
    {
    digitalWrite(13,HIGH);
    //eseguo la prima operazione disponibile (se ce n'è una)
    if(i2c[0][0]==10)           //POV
      {
      cL=i2c[0][1]; //lo zero è 127, con 0 si ha la folle
      cR=i2c[0][2];
      if (cL==0) folleL=true;
      if (cR==0) folleR=true;
      }
    if ((i2c[0][0]>=12)&&(i2c[0][0]<=15))
      {
      strcpy (comando,"MO=1");                //start drive
      scrivi_seriale(comando,i2c[0][0]-12);
      }
    if ((i2c[0][0]>=16)&&(i2c[0][0]<=19))
      {
      strcpy (comando,"MO=0");                //stop drive
      scrivi_seriale(comando,i2c[0][0]-16);
      }
    if (i2c[0][0]==20)
      {
      strcpy(comando,"MO=1");                          //start the motion
      if (folleL==true){
        scrivi_seriale(comando,0);
        scrivi_seriale(comando,2);}
      if (folleR==true){
        scrivi_seriale(comando,1);
        scrivi_seriale(comando,3);}
        
      if(MAF){
      strcpy (comando,"MO=1");                //riabilita i motori se si è in MAF
      scrivi_seriale(comando,0);
      scrivi_seriale(comando,1);
      scrivi_seriale(comando,2);
      scrivi_seriale(comando,3);
      MAF=false;
      }
      
      strcpy (comando,"ST");                //stop all motor
      scrivi_seriale(comando,0);
      scrivi_seriale(comando,1);
      scrivi_seriale(comando,2);
      scrivi_seriale(comando,3);
      flag_moto=false;
      
      velocityL=0;
      velocityR=0;
      folleL=false;
      folleR=false;
      }  
    if ((i2c[0][0]>=21)&&(i2c[0][0]<=26))   //VE
      {
      i=i2c[0][0]-21;
      }  
    if ((i2c[0][0]>=27)&&(i2c[0][0]<=32))  //TE
      {
      byte tempo=(i2c[0][0]-27);                //1 5 10 20 30 60 sec
      switch(tempo){
        case 0:
          {Tm=1*1000;}
          break;
        case 1:
          {Tm=5*1000;}
          break;
        case 2:
          {Tm=10*1000;}
          break;
        case 3:
          {Tm=20*1000;}
          break;
        case 4:
          {Tm=30*1000;}
          break;
        case 5:
          {Tm=60*1000;}
          break;
        }
      }  
    if ((i2c[0][0]>=50)&&(i2c[0][0]<=68))  //EXT
      {
      char comando_esterno[20]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
      int carattere=0;
      while (carattere<i2c[0][0]-49)
        {
          comando_esterno[carattere]=i2c[0][carattere+2]; //+2 perchè in i2c[1] c'è il motore a cui mandare
          carattere++;
        }
      scrivi_seriale(comando_esterno,i2c[0][1]-1);  //aggiungere un metodo di invio della risposta o notifica ricezione
       z=0; 
      while(risposta_ext[z]!='#' && z<31)
             {
              risposta_ext_send[z]=risposta_ext[z];
               z++;
              }
      risposta_ext_send[z]=';';  
      }
    if (i2c[0][0]==34)        //CA2
      Tc=1*1000;
    if (i2c[0][0]==35)
      Tc=2*1000;
    if (i2c[0][0]==36)
      Tc=5*1000;
    if (i2c[0][0]==37)
      Tc=10*1000;
    if (i2c[0][0]==38)
      Tc=30*1000;
    if (i2c[0][0]==39)
      Tc=60*1000; 
    if (i2c[0][0]==40)         //MLM
    {
      MLM=false;
      Tc=10*1000;
      }
    if (i2c[0][0]==41)
      {
      MLM=true;
      Tc=1*1000;
      }
    
    i2c[0][0]=0;
    digitalWrite(13,LOW);
    }
  //fine operazione richieste via I2C  
  
  if ((millis()-Ti>=Tm)&&flag_moto)
    {
    digitalWrite(13,HIGH);
    strcpy(comando,"MO=1");  //riabilita quelli spenti in folle
    if (folleL==true){
        scrivi_seriale(comando,0);
        scrivi_seriale(comando,2);}
    if (folleR==true){
        scrivi_seriale(comando,1);
        scrivi_seriale(comando,3);}
      
    strcpy (comando,"ST");                //stop all motor
    scrivi_seriale(comando,0);
    scrivi_seriale(comando,1);
    scrivi_seriale(comando,2);
    scrivi_seriale(comando,3);
    flag_moto=false;
    
    delay(500);
    
    if(digitalRead(3)==LOW){
      strcpy (comando,"MO=0");                //free motor in MAF
      scrivi_seriale(comando,0);
      scrivi_seriale(comando,1);
      scrivi_seriale(comando,2);
      scrivi_seriale(comando,3);
      MAF=true;
      }
    
    velocityL=0;
    velocityR=0;
    folleL=false;
    folleR=false;
    digitalWrite(13,LOW);
    }
  
  if (millis()-time_count>=Tc) //inibirlo se prima devono spegnersi i motori? ad es per il moto 1 sec
    {
    time_count=millis();
    digitalWrite(13,HIGH);
    leggi_dati_motori();
    digitalWrite(13,LOW);
    } 

}

////////////////////////////////////////  
//            function                //
////////////////////////////////////////  

int leggi_seriale(char comando[],int canale)          //serial read function
{
  int j=0,incomingByte=0,z=0;
  char temp[2][20]; 
  
  switch(canale){
      case 0:
        delay(50);
        if (Serial.available() > 0)                     //if data is avaible on serial 1 buffer, read it
          {
            while((incomingByte=Serial.read())!=-1)     //read the data until stop character
                 {
                 temp[0][j]=incomingByte;
                 //Serial.print(temp[0][j]);
                 j++;
                 }
            temp[0][j]='#';
            //Serial.print(temp[0][j]);
            //Serial.println();            //put new stop character
            j=0;
            z=0;
            while(temp[0][j]!=';')                       //split input string in command (row 0) and response (row 1)
              j++;
            j++;
            while(temp[0][j]!='#')
              {
              temp[1][z]=temp[0][j];
              j++;
              z++;
              }
          }
        break;
      case 1:
        delay(50);
        if (Serial1.available()>0)                     //if data is avaible on serial 1 buffer, read it
          {
            while((incomingByte=Serial1.read())!=-1)     //read the data until stop character
                 {
                 temp[0][j]=incomingByte;
                 //Serial.print(temp[0][j]);
                 j++;
                 }
            temp[0][j]='#';                              //put new stop character
            //Serial.print(temp[0][j]);
            //Serial.println();
            j=0;
            z=0;
            while(temp[0][j]!=';')                       //split input string in command (row 0) and response (row 1)
              j++;
            j++;
            while(temp[0][j]!='#')
              {
              temp[1][z]=temp[0][j];
              j++;
              z++;
              }
          }        
        break;
      case 2:
        delay(50);
        if (Serial2.available() > 0)                     //if data is avaible on serial 1 buffer, read it
          {
            while((incomingByte=Serial2.read())!=-1)     //read the data until stop character
                 {
                 temp[0][j]=incomingByte;
                 //Serial.print(temp[0][j]);
                 j++;
                 }
            temp[0][j]='#';                              //put new stop character
            //Serial.print(temp[0][j]);
            //Serial.println();
            j=0;
            z=0;
            while(temp[0][j]!=';')                       //split input string in command (row 0) and response (row 1)
              j++;
            j++;
            while(temp[0][j]!='#')
              {
              temp[1][z]=temp[0][j];
              j++;
              z++;
              }
          }            
        break;
      case 3:
        delay(50);
        if (Serial3.available() > 0)                     //if data is avaible on serial 1 buffer, read it
          {
            while((incomingByte=Serial3.read())!=-1)     //read the data until stop character
                 {
                 temp[0][j]=incomingByte;
                 //Serial.print(temp[0][j]);
                 j++;
                 }
            temp[0][j]='#';                              //put new stop character
            //Serial.print(temp[0][j]);
            //Serial.println();
            j=0;
            z=0;
            while(temp[0][j]!=';')                       //split input string in command (row 0) and response (row 1)
              j++;
            j++;
            while(temp[0][j]!='#')
              {
              temp[1][z]=temp[0][j];
              j++;
              z++;
              }
          }            
        break;
    }    
  
    
  //command test
    z=0; 
   while((temp[0][z]!='#') && (z<31))
             {
              risposta_ext[z]=temp[0][z];
               z++;
              }
	risposta_ext[z]='#';   
  
  j=1;
  int flag=0;                 //all ok, the command is received correctly
  while (temp[0][j]!=';')
    {
    if (temp[0][j-1]!=comando[j-1])
        {
        flag=1;               //command received incorrect
        }
    j++;  
    }
    
  //response test
    
  j=0;
  strcpy(risposta,";          ");
  while (temp[1][j]!=';')
    {
    risposta[j]=temp[1][j];
    j++;
    }
  if ((temp[1][0]!=';')&&((risposta[0]>='0')&&(risposta[0]<='9')||(risposta[0]=='-')))
    {
    flag=2;                                 //there is a numeric response
    }
  if (risposta[1]=='?')
    {
    flag=risposta[0]+4;                     //unknown elmo exception    aggiungo 4 per eviatre che gli errori 2 e 3 vadano dentro il flag=3,4,5, vedere tabella a pag45 della guida
    time_flag=millis();
    flagC=flag;
    Mo=canale+1;    
    }
  if (strcmp(risposta," ?         ")==0)
    {
    flag=3;      //communication error, re send command
    time_flag=millis();
    flagC=flag;
    Mo=canale+1;
    }    
  if (strcmp(risposta,":?         ")==0)
    {
    flag=4;                                 //motor must be on, start it
    time_flag=millis();
    flagC=flag;
    Mo=canale+1;
    } 
  if ((temp[1][0]=='')&&(temp[1][2]=='')&&(temp[1][3]==''))
    {
    flag=5;                                 //power loss exception, turn off the motor, reconfigure then turn it on
    time_flag=millis();
    flagC=flag;
    Mo=canale+1;
    } 
  return(flag);
}
  
////////////////////////////////////////  

void scrivi_seriale(char comando[], int canale)     //serial write function
  {
  int flag=1;                                       //flag initialization to enter the cicle
  int esci=0;
  strcpy(risposta,"           ");
  char comando_originale[13];
  strcpy(comando_originale,comando);
  
  while((flag==1)||(flag==3))                       //if is the first time or flag is 1 or 3 send/re-send command
    {
    esci++;
    
    if (esci>5)
      {
      flag=255;
      time_flag=millis();
      flagC=flag;
      Mo=canale+1;
      //Serial.println("LOOP errore comunicazione");
      break;  
      }
    
    switch(canale){
      case 0:
        //Serial.println("ELMO 1");
        Serial.print(comando);Serial.write(";");  //compose the string
        flag=int(leggi_seriale(comando,0));         //start the serial read function
        break;
      case 1:
        //Serial.println("ELMO 2");
        Serial1.print(comando);Serial1.write(";");  //compose the string
        flag=int(leggi_seriale(comando,1));         //start the serial read function
        break;
      case 2:
        //Serial.println("ELMO 3");
        Serial2.print(comando);Serial2.write(";");  //compose the string
        flag=int(leggi_seriale(comando,2));         //start the serial read function
        break;
      case 3:
        //Serial.println("ELMO 4");
        Serial3.print(comando);Serial3.write(";");  //compose the string
        flag=int(leggi_seriale(comando,3));         //start the serial read function
        break;
    }    
    
//    Serial.print(comando);                          //send response to the pc throught serial 0
//    Serial.print("|");
//    Serial.print(flag);
//    Serial.print("|");
    
    if (flag==2) //cioè non c'è nessun carattere strano nella risposta
    {
      risposta_long=atol(risposta);                   //convert response string to integer                                                  occhio che resetta arduino se arrivano delle lettere
      risposta_float=atof(risposta);
//      if((strcmp(comando,"MS")==0)||(strcmp(comando,"SR")==0))
//        Serial.print(risposta_long,BIN);
//      else
//        Serial.print(risposta_float);
//      Serial.println();
    }
    
    if (flag==5)  //power loss exception
        {        
        strcpy (comando,"MO=0");                    //motor off
        scrivi_seriale(comando,canale);
        
        configurazione_iniziale(canale);            //reconfigure the motor
        
        strcpy (comando,"MO=1");                    //motor on
        scrivi_seriale(comando,canale);
        
        scrivi_seriale(comando_originale,canale);
        }
    
    if (flag==4) //motor must be on exception
      {
        strcpy(comando,"MO=0");
        scrivi_seriale(comando,canale);  
        delay(50);
        
        strcpy(comando,"MO=1");
        scrivi_seriale(comando,canale); 
      }
      
   if (flag>5) //unknown exception, launch debug function
      {
        strcpy(comando,"MF");                             //motor failure
        scrivi_seriale(comando,canale);  
        
        strcpy(comando,"SR");                             //status register
        scrivi_seriale(comando,canale);  
      }
    }
//  Serial.print("flag->");
//  Serial.println(flagC);
  }
  
///////////////////////////////////////////////////////

void configurazione_iniziale(int canale)                    //motor driver configuration function
  {  
  char comando[13];
    
  strcpy (comando,"AC=29300");                   //acceleration
  scrivi_seriale(comando,canale);
  
  strcpy (comando,"PM=0");                        //profiler mode off
  scrivi_seriale(comando,canale);

  strcpy (comando,"DC=29300");                   //deceleration
  scrivi_seriale(comando,canale);
  
  strcpy (comando,"SD=29300");                   //stop deceleration
  scrivi_seriale(comando,canale);

  strcpy(comando,"SF=0");                         //smooth factor
  scrivi_seriale(comando,canale);
  
  strcpy(comando,"UM=2");                         //unity mode su velocity mode
  scrivi_seriale(comando,canale);
  
  strcpy(comando,"CL[1]=2.5");
  scrivi_seriale(comando,canale);
  
  strcpy(comando,"PL[1]=3.5");
  scrivi_seriale(comando,canale);
  
  strcpy(comando,"PL[2]=10");
  scrivi_seriale(comando,canale);
  
  strcpy(comando,"MO=1");
  scrivi_seriale(comando,canale);
  
  strcpy(comando,"MO=0");
  scrivi_seriale(comando,canale);
  
  }
  
////////////////////////////////////////////

void leggi_dati_motori()
  {
  char comando[13];
  int j=0;
  int m=0;
  byte temporaneo[2];
  
  digitalWrite(13,HIGH);
  //for (j=0;j<4;j++) for (m=0;m<16;m++) dati[j][m]=0;              //reset a 0 di dati
  dati[0][15]=0;   //resetto gli allarmi, togliere se si rimette quello qui sopra
  
  ////dati 1////
  dati[0][0]=int(MaxLinearV[i]);
  dati[0][1]=int(velocityL*3600/CPR*t);                          // ok formula
  dati[0][2]=int(velocityR*3600/CPR*t);
  for (j=0;j<4;j++)
    {
      strcpy(comando,"VX");
      scrivi_seriale(comando,j);
      dati[0][3+j]=int(risposta_float*3600/CPR*t);
    }//6
  for (j=0;j<4;j++)
    {
      strcpy(comando,"IQ");
      scrivi_seriale(comando,j);
      dati[0][7+j]=int(risposta_float*1000);
      if ((dati[0][7+j]>1500)||(dati[0][7+j]<-1500))
        {
        bitSet(dati[0][15],3-j);
        digitalWrite(12,LOW);
        delay(10);
        digitalWrite(12,HIGH);
        }
    }//10
  temporaneo[0]=int(analogRead(12)*(3.332f/1024.0f)*183.282f-257.611f);   //temperature motori kty
//  Serial.println();
//  Serial.println(temporaneo[0]);
    if(temporaneo[0]>90)
        {
        bitSet(dati[0][15],11);
        digitalWrite(12,LOW);
        delay(10);
        digitalWrite(12,HIGH);
        }
  temporaneo[1]=int(analogRead(13)*(3.30f/1024.0f)*183.282f-257.611f);
//  Serial.println(temporaneo[1]);
  if (temporaneo[1]>90)
        {
        bitSet(dati[0][15],10);
        digitalWrite(12,LOW);
        delay(10);
        digitalWrite(12,HIGH);
        }
  bytesToInteger(temporaneo,2, &dati[0][11]);                         //compone dato
  temporaneo[0]=0;//int(analogRead(14)*(3.332f/1024.0f)*183.282f-257.611f);
//  Serial.println(temporaneo[0]);
  if (temporaneo[0]>90)
        {
        bitSet(dati[0][15],9);
        digitalWrite(12,LOW);
        delay(10);
        digitalWrite(12,HIGH);
        }
  temporaneo[1]=int(analogRead(15)*(3.332f/1024.0f)*183.282f-257.611f);
//  Serial.println(temporaneo[1]);
  if (temporaneo[1]>90)
        {
        bitSet(dati[0][15],8);
        digitalWrite(12,LOW);
        delay(10);
        digitalWrite(12,HIGH);
        }  
  bytesToInteger(temporaneo,2, &dati[0][12]);                         //compone dato
  //i tempi li aggiorna alla fine
  temporaneo[0]=flagC;
  temporaneo[1]=Mo;
  bytesToInteger(temporaneo,2, &dati[0][14]); //mettere ultimo flag critico  
  if (flag==3)
      {
      bitSet(dati[0][14],7);
      digitalWrite(12,LOW);
      delay(10);
      digitalWrite(12,HIGH);
      }
  if (flag==5)
      {
      bitSet(dati[0][14],5);
      digitalWrite(12,LOW);
      delay(10);
      digitalWrite(12,HIGH);
      }
  if (flag>5)
      {
      bitSet(dati[0][14],6);
      digitalWrite(12,LOW);
      delay(10);
      digitalWrite(12,HIGH);
      }
  for (j=0;j<4;j++)
    {
      strcpy(comando,"TI[1]");
      scrivi_seriale(comando,j);
      if (risposta_float>70)
        {
        bitSet(dati[0][15],15-j);
        digitalWrite(12,LOW);
        delay(10);
        digitalWrite(12,HIGH);
        }
    }
  ////dati 2////
  if(!MLM)      //non fa dati 2 se è in log rapido
    {
    for (j=0;j<4;j++) 
      {
        strcpy(comando,"MF");
        scrivi_seriale(comando,j);
        dati[1][2*j+1]= int(risposta_long >> 16);
  		dati[1][2*j]= int(risposta_long & 0xffff);
      }//7
    for (j=0;j<4;j++) 
      {
        strcpy(comando,"SR");
        scrivi_seriale(comando,j);
        dati[1][2*j+9]=int(risposta_long >> 16);
  		dati[1][2*j+8]=int(risposta_long & 0xffff);
      }//15
    }
  //allarmi temperature drive
  digitalWrite(13,LOW);
  }

////////////////////////////////////////


void receiveEvent(int num)
{
  //delay(200);
  delay(50);
  int z=0;
  while(Wire.available())
    {
    i2c[1][z]=Wire.read();    
    if (z>18) break;
    z++;
    }
  if (((i2c[0][0]>=16)&&(i2c[0][0]<=19))||(i2c[0][0]==20))
    {
    //ignora comando nuovo fino a che non è eseguito quello prioritario
    }
  else
    {
    z=0;
    while(z<20)
      {
      i2c[0][z]=i2c[1][z];
      z++;
      }
    }
  Wire.flush();
  //delay(200);
  //delay(50);
}

////////////////////////////////////////////////////////////////// 

void requestEvent()
{
  byte temporaneo[2];
  int z=0;
  delay(50);//delay(200);
  switch (i2c[0][0])
  {
    case 0:
          temporaneo[0]=byte((millis()-time_count)/1000);           //in sec
          temporaneo[1]=byte((millis()-time_flag)/1000);
          bytesToInteger(temporaneo,2, &dati[0][13]); 
          NUM_DATI=16;
          invia_i2c(dati[0],NUM_DATI);
          i2c[0][0]=0;
          //flagC=0;
          //Mo=0;   
    break;
    case 1:
          NUM_DATI=16;
          invia_i2c(dati[1],NUM_DATI);
          i2c[0][0]=0;
    break;
    case 3: 
          Wire.write(risposta_ext_send,32);
    break;
  }
  //delay(50);//delay(200);
}

//////////////////////////////////////////////////////

void bytesToInteger(byte *b,byte ByteNumber, int *dato) 
{
  int k=0;
  for (k=0; k<ByteNumber/2;k++)
  {  
	  dato[k] = ((int )b[2*k]) << 8;
	  dato[k] |= b[2*k+1];
  }	  
}

/////////////////////////////////////////////////////////////

void integerToBytes(int val, byte b[2]) {
  b[0] = (byte ((val >> 8) & 0xff));
  b[1] = (byte(val & 0xff));
}

//////////////////////////////////////////////////////////////

void invia_i2c (int *dato, byte num_dati)
{
  byte i=0;
  byte one[num_dati*2];
  for (i=0;i<num_dati; i++)
  {
    integerToBytes(dato[i], &one[i*2]);
  }
   Wire.write(one,num_dati*2);
}

///////////////////////////////////////////////////////////////

